#include "MeshlessSkinning.h"
#include "MeshlessSkinning/CIsoSurface.h"
#include <vector>
#include <fstream>
using namespace std;

namespace GeoLeo{

void MeshlessSkinning::levelSetSkinning(const string& infname, const string& outfname, const double resolution, const double max_min_ratio, const int flags)
{

    int iter_laplace = 15;
    int iter_biharmonic = 100;
    int redistance_frequency = 50;

    double rmin = -DBL_MAX, rmax = -DBL_MAX, rinit = -DBL_MAX, vel_gain = 1.0,
        dt_laplace = -DBL_MAX, dt_biharmonic = -DBL_MAX, dt_biharmonic_gain = 1.0;
    double max_stretch = 1.0, rratio = max_min_ratio;

    std::vector<SlVector3> particles, velocities;
    std::vector<double> radii;

    double h = resolution;

    if (rmin == -DBL_MAX) {
        rmin = 0.86603 * h; // 0.5*sqrt(3)*h
                            // there could be a stretch up to this amount, we want to make sure that particles
                            // all touch at least one grid point, so make the radius bigger...
        if (flags & SmoothingGrid::NEIGHBOR_ANISOTROPY) {
            rmin *= sqrt(max_stretch);
        }
        else if (flags & SmoothingGrid::VELOCITY_ANISOTROPY) {
            rmin *= cbrt(max_stretch);
        }
    }

    if (rmax == -DBL_MAX) rmax = rratio * rmin;
    if (rinit == -DBL_MAX) rinit = 0.5*(rmin + rmax);
    else if (!(flags & SmoothingGrid::VARIABLE_RADIUS)) rinit *= rmin;
    if (dt_laplace == -DBL_MAX) dt_laplace = 0.1*h*h;
    if (dt_biharmonic == -DBL_MAX) dt_biharmonic = 0.01*dt_biharmonic_gain*h*h*h*h;

    ifstream input(infname.c_str(), std::ios::in | std::ios::binary);
    int nparticles;
    float minx = 100000.0f;
    float maxx = -100000.0f;
    float miny = 100000.0f;
    float maxy = -100000.0f;
    float minz = 100000.0f;
    float maxz = -100000.0f;
    float x, y, z;
    input.read((char*)&nparticles, sizeof(int));
    float fdummy;
    for (int i = 0; i < nparticles; i++)
    {
        input.read((char*)&x, sizeof(float));
        input.read((char*)&y, sizeof(float));
        input.read((char*)&z, sizeof(float));

        //input.read((char*)&fdummy, sizeof(float));

        if (x < 2.0 && x > -1.0 && y < 2.0 && y > -1.0f && z < 2.0 && z > -1.0)
        {
            particles.push_back(SlVector3(x, y, z));
        }

    }

    SmoothingGrid grid(h, rmin, rmax, rinit, vel_gain, max_stretch, flags, particles, radii, velocities);

    grid.doLaplacianSmoothing(iter_laplace, dt_laplace, redistance_frequency);

    grid.doBiharmonicSmoothing(iter_biharmonic, dt_biharmonic, redistance_frequency);

    dumpSurfaceToObj(outfname, grid, 0);

    particles.clear();
    radii.clear();
    velocities.clear();
}

void MeshlessSkinning::dumpSurfaceToObj(const std::string & outfname, SmoothingGrid & grid, double shrink)
{
    std::vector<SlTri> triangles; // triangles in polygon mesh
    std::vector<SlVector3> meshPts; // points in polygon mesh
    std::vector<SlVector3> normals; // normals (at points) in polygon mesh

    CIsoSurface<float> mc;

    int nx = grid.nx;
    int ny = grid.ny;
    int nz = grid.nz;

    SlArray3D<float> phi(nz, ny, nx);
    for (int i = 0; i < phi.nx(); i++)
    {
        for (int j = 0; j < phi.ny(); j++)
        {
            for (int k = 0; k < phi.nz(); k++)
            {
                phi(i, j, k) = (float)(grid.phi(k, j, i));
            }
        }
    }

    //needs to be modified
    mc.GenerateSurface(phi.data, 0.0, nx - 1, ny - 1, nz - 2, (float)grid.h, (float)grid.h, (float)grid.h);

    for (unsigned int i = 0; i<mc.m_nVertices; i++)
    {
        POINT3D& pt = mc.m_ppt3dVertices[i];
        meshPts.push_back(SlVector3(pt[0] + grid.bbMin[0], pt[1] + grid.bbMin[1], pt[2] + grid.bbMin[2]));
    }

    for (unsigned int i = 0; i<mc.m_nTriangles; i++) {
        triangles.push_back(SlTri(mc.m_piTriangleIndices[3 * i], mc.m_piTriangleIndices[3 * i + 1], mc.m_piTriangleIndices[3 * i + 2]));
    }

    constructNormals(normals, meshPts, triangles);

    std::cout << "Start saving mesh.\n";
    std::ofstream output(outfname, std::ios::out);
    std::vector<SlVector3>::const_iterator p;
    std::vector<SlTri>::const_iterator t;
    std::vector<SlVector3>::const_iterator n;

    int nv = meshPts.size();
    int nf = triangles.size();


    for (int i = 0; i<nv; i++) {
        output << "v " << meshPts[i][0] << " " << meshPts[i][1] << " " << meshPts[i][2] << endl;
    }
    //	output << "usemtl (null)" << endl;
    //	output << "s off" << endl;

    // 	for (int i=0; i<nv; i++) {
    // 		output << "vn " << normals[i][0] << " " << normals[i][1] << " " << normals[i][2] << endl;
    // 	}

    for (int i = 0; i<nf; i++) {
        output << "f " << triangles[i][0] + 1 << " " << triangles[i][1] + 1 << " " << triangles[i][2] + 1 << endl;
    }
    output << endl << endl;

    output.close();

    triangles.clear();
    meshPts.clear();
    normals.clear();
}

void MeshlessSkinning::constructNormals(std::vector<SlVector3>& normals, std::vector<SlVector3>& vertices, std::vector<SlTri>& faces)
{
    int nf = faces.size();
    int nv = vertices.size();
    normals.resize(nv);
    std::vector<SlVector3> aveN;
    std::vector<int> stat;
    aveN.resize(nv);
    stat.resize(nv);
    for (int i = 0; i < nv; i++)
    {
        normals[i].set(0.0);
        aveN[i].set(0.0);
    }

    for (int i = 0; i < nf; i++)
    {
        int v1 = faces[i][0];
        int v2 = faces[i][1];
        int v3 = faces[i][2];
        SlVector3 e1 = vertices[v1] - vertices[v2];
        SlVector3 e2 = vertices[v3] - vertices[v2];
        SlVector3 n = cross(e2, e1);
        normalize(n);
        aveN[v1] += n;
        aveN[v2] += n;
        aveN[v3] += n;
    }
    for (int i = 0; i < nv; i++)
    {
        normalize(aveN[i]);
    }

    for (int i = 0; i < nf; i++)
    {
        int v1 = faces[i][0];
        int v2 = faces[i][1];
        int v3 = faces[i][2];

        normals[v1] += aveN[v2];
        normals[v1] += aveN[v3];
        normals[v2] += aveN[v1];
        normals[v2] += aveN[v3];
        normals[v3] += aveN[v1];
        normals[v3] += aveN[v2];
    }
    for (int i = 0; i < nv; i++)
    {
        normalize(normals[i]);
    }

    aveN.clear();
    stat.clear();
}

}
